// main.c
// Written By Team Flutterbot
// standard libs
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>


#include "main.h"
#include "ff_utils.h"
#include "../utils/LCD_driver.h"
#include "../utils/LCD_functions.h"
#include "../utils/util.h"
#include "../utils/button.h"

extern volatile char KEY_VALID;


// line sensors are connected to ADC4(PF4) thru ADC7(PF7).  Port F is also known as the JTAG port and 
// JTAG must be disabled inorder to use these pins.  The pins are not numbered the same as the other connectors.
//                              +---+---+---+---+---+
//            Port F  (JTAG)    | X | X | X | X | X |  Pins marked with an X should not be used.  
//            Usable Pins       +---+---+---+---+---+
//                              | 7 | X | 5 | 6 | 4 |  
//                              +---+---+---+---+---+
//PF4 - RightDistSensor
//PF6 - MiddleDistSensor
//PF5 - LeftDistSensor 	
//PF7 - FrontLineSensor 


// Motor control circuit is connected to PortB  
//                              +---+---+---+---+---+
//            Port B            | 1 | 3 | 5 | X | V |  Pins marked with an X are not be used.  
//     Motor Contorl pins       +---+---+---+---+---+
//                              | 0 | 2 | x | x | G |  
//                              +---+---+---+---+---+
//  PB0 - Left motor  pin 2 on IXDN404 (yellow)
//  PB1 - Right motor pun 2 on IXDN404 (blue)
//  PB2 - Left motor  pin 4 on IXDN404 (green)
//  PB3 - Right motor pin 4 on IXDN404 (orange)

//	PB5 - pin 6, 0x20, balloon pop sensor


int main (void)
{ 
    Initialization();   //
	
	USART_Init(51);
	
	I2C_Init();			// Inits I2C Routines
	
    SetupLineSensors();
	
    InitADC();
	
	printLCD("FF v3.02");
	sendString("Firefight Version 4.00\r\r"); 

	
	//unsigned char	OriginalHeading;
	//OriginalHeading = 0x7A;
   
	while(1)
	{
	
	printLCD("Main");
	
    while (!KEY_VALID);
	
    if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
			printLCD("Home Room 1");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ToRoom1();
				printLCD("Back");
				TimerWait(500);
			}		

			printLCD(" 1 Room 2");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ToRoom2();
				printLCD("Back");
				TimerWait(500);
			}	

			printLCD("2 Room 3");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ToRoom3();
				printLCD("Back");
				TimerWait(500);
			}

			printLCD("3 Room 4");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ToRoom4();
				printLCD("Back");
				TimerWait(500);
			}

			printLCD("Exit Room");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ExitRoom();
				printLCD("Back");
				TimerWait(500);
			}		

	
			printLCD("L wall follow");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0X15);
				SetLeftMotorPWM(0);
				SetRightMotorPWM(0);
				printLCD("Back");
				TimerWait(500);				
			}		

			printLCD("R wall follow");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				RWallFollow(0x30, UNTIL_LINESENSOR_LOWER,0X15);
				SetLeftMotorPWM(0);
				SetRightMotorPWM(0);
				printLCD("Back");
				TimerWait(500);	
			}	

			printLCD("Home F 1");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HomeFrom1();
				printLCD("Back");
				TimerWait(500);
			}			

			printLCD("Home F 2");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HomeFrom2();
				printLCD("Back");
				TimerWait(500);
			}			

			printLCD("Home F 3");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HomeFrom3();
				printLCD("Back");
				TimerWait(500);
			}			

			printLCD("Home F 4");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HomeFrom4();
				printLCD("Back");
				TimerWait(500);
			}			
        }
	printLCD("Test");

    while (!KEY_VALID);
    if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {	
			printLCD("Go To Flame");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				GoToFlame();
				printLCD("Back");
				TimerWait(500);
			}		

			/*printLCD("Linear Distance ACDs");   // Display text that is in quotes on the LCD
			while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);					// delay while letting go!
				showLinearDist();                   
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}*/

			/*printLCD("NON-Linear Distance ACDs");   // Display text that is in quotes on the LCD
			while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);					// delay while letting go!
				showDist();                   
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}*/

			printLCD("Scan For Flame");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ScanRoomForFlame();
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				printLCD("Back");
				TimerWait(500);
			}	

			/*printLCD("Display Flame Sensor");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				DisplayFlame();
				printLCD("Back");
				TimerWait(500);
			}*/				

			/*printLCD("Heading Test");
			while (!KEY_VALID);
			if (getkey() == 1)     // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HeadingTest();
				printLCD("Back");
				TimerWait(500);
			}*/			

			/*printLCD("PWM Test");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				PWM_Test();
				printLCD("Back");
				TimerWait(500);
			}*/

			/*printLCD("Line Display");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				LineDisplay();
				printLCD("Back");
				TimerWait(500);
			}*/

			/*printLCD("L R DistTest");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				showADCLeftRight();
				printLCD("Back");
				TimerWait(500);
			}*/				

			/*printLCD("Line & C-Dist Test");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				showADCLineCenter();
				printLCD("Back");
				TimerWait(500);
			}*/	

			/*printLCD("Compass Diff");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				CompassDiff();
				printLCD("Back");
				TimerWait(500);
			}*/					

			/*printLCD("Therm Noise Test");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ThermNoise();
				printLCD("Back");
			}*/

			/*printLCD("Comp Noise Test");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				CompNoise();
				printLCD("Back");
			}*/		

			/*printLCD("Flame Sensor Test");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				FlameSensorTest();
				printLCD("Back");
				TimerWait(500);
			}*/

			printLCD("Compass Test");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				CompassTest();
				printLCD("Back");
				TimerWait(500);
			}	
		}
	/*printLCD("Example");
    while (!KEY_VALID);
    if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
			printLCD("Example");
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				//Example();
				printLCD("Back");
				TimerWait(500);
			}
		}*/
	}
}

